/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package org.sunspotworld;

import com.sun.spot.sensorboard.EDemoBoard;
import java.io.IOException;
import java.util.*;

/**
 *
 * @author Administrator
 */
public class MotorController {
    
    // commands
    // int enums, enum not supported in this version
    public static final int FORWARD         = 65;
    public static final int REVERSE         = 66;
    public static final int TURN_LEFT       = 67;
    public static final int TURN_RIGHT      = 68;
    public static final int ROTATE_LEFT     = 69;
    public static final int ROTATE_RIGHT    = 70;
    public static final int INC_LEFT_SPEED  = 71;
    public static final int DEC_LEFT_SPEED  = 72;
    public static final int INC_RIGHT_SPEED = 73;
    public static final int DEC_RIGHT_SPEED = 74;
    public static final int ROTATE_LEFT_N   = 75;
    public static final int ROTATE_RIGHT_N  = 76;
    public static final int REVERSE_N       = 79;
    public static final int TURN_LEFT_N     = 81;
    public static final int TURN_RIGHT_N    = 82;
    public static final int TURN_LEFT_REV_N     = 77;
    public static final int TURN_RIGHT_REV_N    = 78;
    public static final int FORWARD_N       = 83;
    public static final int BRAKE           = 80;
    public static final int STRAIGHT           = 86;
    public static final int ADJ_LEFT           = 87;
    public static final int ADJ_RIGHT           = 88;
    public static final int SET_TICK_SPEED  = 89;
    
    
    public int lastCommand = -999;
    public int lastSpeed = -999;
    int commandDelay = 100;
    long lastCommandTime;
    public int leftSpeed;
    public int rightSpeed;
    
    EDemoBoard demo = EDemoBoard.getInstance();
    
    public MotorController(){
        lastCommandTime = System.currentTimeMillis() + 1000;
    }
    
    public boolean sendCommand( int command, int speed ){
        if( speed < 1 ){
            System.out.println("Received a bad speed in the motor controller returning");
            return true;
        }
        
        // keep this if we need to do something about the multiple rotates.
        if( lastCommand == command && ( command == this.ROTATE_LEFT_N || command == this.ROTATE_RIGHT_N ) && speed >= 10 ){
            System.out.println("Received two rotate commands in a row... going to sleep and returning true.");
            this.toSleep(1000);
            return true;
        }
       // if( !(command == this.ROTATE_RIGHT_N || command == this.ROTATE_LEFT_N || command == this.STRAIGHT || command == this.SET_TICK_SPEED) ) return;
       // if( command == 89 ) speed = 70;
       // if( command >= 75 && command <= 83 ){
            //if( speed > 20 ) speed = 20;
        //}
        // TODO remove me
        //if( 1 == 1 )
        //    return;
        
        /*
        while(lastCommandTime+commandDelay > System.currentTimeMillis()){
            try{
                Thread.sleep(10);
            } catch(Exception e) {}
        }
         */
         
            
        lastCommandTime =  System.currentTimeMillis();
        if( command == FORWARD || command == REVERSE  ){
            this.lastSpeed = speed;
            this.leftSpeed = speed;
            this.rightSpeed = speed;
        }
        
        
        
        //System.out.println("MotorCommand: " + command + " Speed: " + speed );       
        
        flushBuffer();
        int recCommand = 0;
        int recSpeed = 0;
        boolean commandAck = false;
        byte ackByte = 0;
        while( !commandAck ){
            demo.writeUART((byte)0x21);
            demo.writeUART((byte)command);
            flushBuffer();
            demo.writeUART((byte)speed);
            
            System.out.println("Sending: "+(byte)0x21+","+(byte)command+","+(byte)speed);
            try{
                Thread.sleep(100);
                ackByte = demo.readUART(10000);
                recCommand = demo.readUART(100);
                recSpeed = demo.readUART(100);
                System.out.println("Ack: "+(byte)ackByte+","+(byte)recCommand+","+(byte)recSpeed);
            }catch(Exception e){}
            
            if( ackByte == '"' )
                commandAck = true;
            else
                return false;
            
        }
        
        
        // block until we recieve the command back.  Change this to have a timeout
        byte data[] = new byte[3];
        int counter = 0;
        try{
            Thread.sleep(100);
            System.out.print("Receiving: ");
            data[0] = (byte)demo.readUART(8000);
            data[1] = (byte)demo.readUART(100);
            data[2] = (byte)demo.readUART(100);
             
            /*
            System.out.print("Receiving: ");
            System.out.print((byte)demo.readUART(5000)+",");
            System.out.print((byte)demo.readUART(100)+",");
            System.out.println((byte)demo.readUART(100));
            */
        }catch(Exception e){}
        
        if( data[1] != recCommand || data[2] != recSpeed){
            System.out.print("I did not receive a command complete!");
            return false;
        }
        
        flushBuffer();
        if( command < 71 || command > 74 ){
            this.lastCommand = command;
            
        }
        return true;
    }
    
    public void adjustLeft(){
        
            this.sendCommand(this.ADJ_RIGHT, 3);

    }
    
    public void adjustRight(){

            this.sendCommand(this.ADJ_LEFT, 3);


    }
    
    private byte receiveData(){
        try{
            return demo.readUART();
        }catch(IOException e){}
            return (byte)0xFF;
    }
    
    private void flushBuffer(){
        //flush receive buffer.
         try
         {
            while(demo.availableUART() > 0)
               demo.readUART(100);
         }
         catch(IOException ioe) {} 
    }
    
    private void toSleep( int msec ){
         try {
                Thread.sleep(msec);
            } catch (InterruptedException e) {
                //We've been interrupted: exit the thread
                return;
            }
        
    }

}
